
#include "common.h"

bool timePassed(unsigned long &t, int32_t diff) {
    unsigned long t1 = millis();
    if (t1 - t >= diff) {
        t = t1;
        return true;
    }
    return false;
}

void printHex(std::string data) {
    for (int i = 0; i < data.length(); i++) {
        Serial.printf("%02X ", data[i]);
    }
}

void printHex(uint8_t *data, int len) {
    for (int i = 0; i < len; i++) {
        Serial.printf("%02X ", data[i]);
    }
}

void printBin(void *data, int len) {
    char buf[9];
    uint8_t *pdata = (uint8_t *)data;
    for (int i = 0; i < len; i++) {
        itoa(pdata[i], buf, 2);
        for (int j = strlen(buf); j < 8; j++) {
            Serial.print('0'); // 补零到8位
        }
        Serial.print(buf);
        Serial.print(" ");
    }
}

int binToInt(void *data, int size) {

    switch (size) {
    case 1:
        return *((int8_t *)data);
    case 2:
        return *((int16_t *)data);
    default:
        return *((int32_t *)data);
    }
}

// 从 src的偏移offsetSrc位，复制len位到tar
// 不满padding位的，以最后一个bit补满
// 偏移量offsetSrc超过src的长度（srcLen）时，全部填0
void bincpy(void *tar, void *src, int offsetSrc, int len, int padding, int srcLen) {
    uint8_t *pTar = (uint8_t *)tar;
    uint8_t *pSrc = (uint8_t *)src;

    pTar--;
    pSrc += offsetSrc >> 3;
    offsetSrc &= 7;
    uint8_t c = *pSrc;
    if (offsetSrc) {
        c >>= offsetSrc;
    } else {
        pSrc--;
    }

    int last = 0;
    int i = 0;

    if (offsetSrc <= srcLen) {
        for (; i < len; i++, offsetSrc++) {

            if ((i & 7) == 0) {
                *(++pTar) = 0;
            }
            if ((offsetSrc & 7) == 0) {
                c = *(++pSrc);
            }

            *pTar >>= 1;
            if (c & 0x01) {
                last = 1;
                *pTar |= 0x80;
            } else {
                last = 0;
            }

            c >>= 1;
        }
    }

    // 填充满
    while (i < padding) {
        int n = i & 7;
        if (n == 0) {
            if (last) {
                *(++pTar) = 0xFF;
            } else {
                *(++pTar) = 0;
            }
            i += 8;
        } else {
            int8_t *pTar2 = (int8_t *)pTar;
            *pTar2 >>= 8 - n;
            pTar++;
            i += (8 - n);
        }
    }
}